/**
 * @file mec.hpp
 * @author Linfu Wei (ghowoght@qq.com)
 * @brief 只考虑相对位置的机械编排
 * @version 1.0
 * @date 2022-01-12
 * 
 * @copyright Copyright (c) 2022
 * 
 */

#ifndef _MECHANIZATION_HPP
#define _MECHANIZATION_HPP

#include <iostream>
#include <memory>
#include <eigen3/Eigen/Dense>
#include "nav_base.hpp"


class Mec:NavBase{
private:
    StatePtr state_;

    std::ofstream result_fs; 
public:
    bool ready;

public:
    Mec() = default;
    Mec(StatePtr& state){
        state_ = state;
        ready = false;
        result_fs = std::ofstream("/home/ghowoght/workspace/meg-odom/python/config/17floor/data-17floor-1-10/result/result_ekf.txt");
    }
    void update(ImuData& imudata){

        double dt = imudata.timestamp - state_->timestamp;
        state_->timestamp = imudata.timestamp;
        Eigen::Vector3d gn;
        // gn << 0, 0, -9.789198777434285;
        gn << 0, 0, -9.7932; // robot
        // 姿态更新
        Eigen::Vector3d rotvec = imudata.gyro_rps * dt;
        Eigen::Vector4d temp;        
        temp << sin(0.5 * rotvec.norm()) / (0.5 * rotvec.norm()) * 0.5 * rotvec, 
                cos(0.5 * rotvec.norm());
        Eigen::Quaterniond q_bb(temp);
        state_->q_bn *= q_bb;
        state_->q_bn.normalize();
        quat2euler(state_->q_bn, state_->att);
        quat2dcm(state_->q_bn, state_->c_bn);
        // 速度更新
        state_->vel += (state_->c_bn * (imudata.accel_mpss) - gn) * dt;
        // imudata.accel_mpss[1] = 0;
        auto tt = state_->c_bn * (imudata.accel_mpss) - gn;
        // std::cout << "-----------------------------   " << tt[0] << " " << tt[1] << " " << tt[2] << std::endl;
        // state_->vel[2] = 0;
        // 位置更新
        state_->pos += state_->vel * dt;


        // 保存结果
        result_fs << state_->timestamp << " " 
            << state_->pos[0] << " " << state_->pos[1] << " " << state_->pos[2] << " "
            << state_->vel[0] << " " << state_->vel[1] << " " << state_->vel[2] << " "
            // << tt[0] << " " << tt[1] << " " << tt[2] << " "
            << state_->att[0] * r2d << " " << state_->att[1] * r2d << " " << state_->att[2] * r2d << " " 
            << std::endl;

        std::cout << "----------------------" << state_->timestamp << std::endl;
        std::cout << "att: " << state_->att[0] * r2d << " " << state_->att[1] * r2d << " " << state_->att[2] * r2d << " " << std::endl;
        std::cout << "pos: " << state_->pos[0] << " " << state_->pos[1] << " " << state_->pos[2] << " " << std::endl;
        std::cout << "vel: " << state_->vel[0] << " " << state_->vel[1] << " " << state_->vel[2] << " " << std::endl;
    } 
};

#endif
